using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using NetduinoHappyHour.Util;
using SecretLabs.NETMF.Hardware;
using SecretLabs.NETMF.Hardware.Netduino;
using System;
using System.Threading;

namespace NetduinoHappyHour.Hardware
{
    /// <summary>
    /// Basic drive command for dc motors to be executed on the robot
    /// Written by: Salvador Richter
    /// Appache License Version 2.0 
    /// </summary>
    public class DriveCommandDC : IConstructors, IDriveCommand
    {
        /// <summary>
        /// The duty cycle of the left pwm signal, range from 0 to 100
        /// </summary>
        public uint PwmDutyCycleL { get; private set; } // TODO override set to prevent out of range values

        /// <summary>
        /// The duty cycle of the left pwm signal, range from 0 to 100
        /// </summary>
        public uint PwmDutyCycleR { get; private set; } // TODO override set to prevent out of range values

        private uint _timeout;

        /// <summary>
        /// Runs for Timeout seconds with zero pwm power
        /// To really run the PwmDutyCycleL and PwmDutyCycleR needs to be set
        /// Timeouts after uint.MaxValue seconds
        /// </summary>
        public DriveCommandDC()
        {
            if (SystemConfig.Instance().UseAdaFruitForDcMotorDriving)
            {
                InitAdaFruitShield();
            }
            PwmDutyCycleL = 0;
            PwmDutyCycleR = 0;
            _timeout = uint.MaxValue;
        }

        /// <summary>
        /// Runs for Timeout seconds with the given pwm power
        /// Timeouts after uint.MaxValue seconds
        /// </summary>
        /// <param name="PwmL">PwmDutyCycle left</param>
        /// <param name="PwmR">PwmDutyCycle right</param>
        public DriveCommandDC(uint PwmL, uint PwmR)
            : this()
        {
            PwmDutyCycleL = PwmL;
            PwmDutyCycleR = PwmR;
        }

        /// <summary>
        /// Runs for Timeout seconds with the given pwm power
        /// </summary>
        /// <param name="PwmL">PwmDutyCycle left</param>
        /// <param name="PwmR">PwmDutyCycle right</param>
        /// <param name="Timeout">seconds to run</param>
        public DriveCommandDC(uint PwmL, uint PwmR, uint Timeout)
            : this()
        {
            PwmDutyCycleL = PwmL;
            PwmDutyCycleR = PwmR;
            _timeout = Timeout;
        }

        public String[] GetConstructors()
        {
            return new String[] { "()", "(uint PwmL, uint PwmR)", "(uint PwmL, uint PwmR, uint Timeout)" };
        }

        public void Execute()
        {
            PWM left = new PWM(SystemConfig.Instance().DCMotorPwmLeft); // M3
            PWM right = new PWM(SystemConfig.Instance().DCMotorPwmRight); // M4
            uint period = 10000;    // 10 ms --> to be modified for requirements of used motors
            uint t=0;

            while( t<_timeout )
            {
                left.SetPulse(period, (period / 100) * PwmDutyCycleL);
                right.SetPulse(period, (period / 100) * PwmDutyCycleR);

                t++;
                Thread.Sleep(1000);
            }

            left.SetPulse(period, 0);
            right.SetPulse(period, 0);

            left.Dispose();
            right.Dispose();
        }

        private void InitAdaFruitShield()
        {
            // M3A = 1; M4A = 32
            ShiftByteToAdaFruitShield(33);
        }

        private void ShiftByteToAdaFruitShield(int Val)
        {
            if (SystemConfig.Instance().IsPinRegistered(Pins.GPIO_PIN_D7))
            {
                // already initialized, no init required
                return;
            }

            int shiftVal = 0;
            OutputPort dirSer = new OutputPort(Pins.GPIO_PIN_D8, false); // 74hct595n pin 14
            OutputPort dirClk = new OutputPort(Pins.GPIO_PIN_D4, false); // 74hct595n pin 11
            OutputPort dirLatch = new OutputPort(Pins.GPIO_PIN_D12, false); // 74hct595n pin 12
            // do not dispose motorEnable
            OutputPort motorEnable = new OutputPort(Pins.GPIO_PIN_D7, false);
            SystemConfig.Instance().RegisterPin(Pins.GPIO_PIN_D7, GetType().Name);
            motorEnable.Write(false);
            dirSer.Write(false);
            dirClk.Write(false);
            dirLatch.Write(false);
            /* precondition: latch pin, data pin and clock pin are all low */
            for (int i = 0; i < 8; i++) // shift 8 bits
            {
                shiftVal = Val & 0x80;
                // put data bit, most significant bit first
                if (shiftVal != 0)
                {
                    dirSer.Write(true);
                }
                else
                {
                    dirSer.Write(false);
                }
                dirClk.Write(true); // CLK high: data gets transfered into memory
                dirClk.Write(false); // CLK low: reset clock
                dirSer.Write(false); // data line low
                Val <<= 1; // shift bit
            }

            dirLatch.Write(true);
            dirLatch.Write(false);

            dirLatch.Dispose();
            dirSer.Dispose();
            dirClk.Dispose();
        }
    }
}
